#include "parkclark.h"
#include <math.h>

// current:abc->alpha_beta
void clark_transform(abc_current_handle abc_ptr,alphabeta_current_handle alphabeta_ptr)
{
    alphabeta_ptr->ialpha = (abc_ptr->ia - 0.5f*abc_ptr->ib-0.5f*abc_ptr->ic)*2.0f/3.0f; 
    alphabeta_ptr->ibeta = (sqrtf(3.0f)/2.0*abc_ptr->ib-sqrtf(3.0f)/2.0*abc_ptr->ic)*2.0f/3.0f;
    return;
}

// current:alpha_beta->q_d
void park_transform(dq_current_handle dq_ptr,alphabeta_current_handle alphabeta_ptr,float electrical_angle)
{
    electrical_angle = electrical_angle * M_PI / 180.0f;
    dq_ptr->id = alphabeta_ptr->ialpha*cosf(electrical_angle)+alphabeta_ptr->ibeta*sinf(electrical_angle);
    dq_ptr->iq = -alphabeta_ptr->ialpha*sinf(electrical_angle)+alphabeta_ptr->ibeta*cosf(electrical_angle);
    return;
}

// voltage:q_d->alpha_beta
void park_inverse_transform(dq_voltage_handle dq_ptr,alphabeta_voltage_handle alphabeta_ptr,float electrical_angle)
{
    electrical_angle = electrical_angle * M_PI / 180.0f;
    alphabeta_ptr->ualpha = dq_ptr->ud*cosf(electrical_angle)-dq_ptr->uq*sinf(electrical_angle);
    alphabeta_ptr->ubeta = dq_ptr->ud*sinf(electrical_angle)+dq_ptr->uq*cosf(electrical_angle);
    return;
}